package pathAlgorithms;

import java.util.HashSet;
import java.util.PriorityQueue;
import java.util.Set;

import javax.vecmath.Point2d;

import program.RoboMap;

public class RoboGraph {
	
	public static final int maybeObjectFactor = 100;

	protected PriorityQueue<RoboPath> queue;
	protected RoboMap roboMap;

	protected Set<Point2d> visited;

	protected GraphLogger graphLogger;
	
	public static int xSize = 10;
	public static int ySize = 10;

	public static double stepSize = 1;
	public static double ACCEPT_DISTANCE = 1;

	public final double time;

	public RoboGraph(RoboMap map) {
		this(map, 0);
	}

	public RoboGraph(RoboMap map, double time) {
		super();
		queue = new PriorityQueue<RoboPath>();
		visited = new HashSet<Point2d>();
		roboMap = map;
		this.time = time;
		graphLogger = new GraphLogger();
	}

	private void addValidNeighBoursToQueue(RoboNode currentNode, RoboPath path,
			RoboNode goalNode) {

		double x = currentNode.getPosition().x;
		double y = currentNode.getPosition().y;

		RoboNode nodeUp = addNodeIfValid(x, y + stepSize, path, goalNode);
		RoboNode nodeDown = addNodeIfValid(x, y - stepSize, path, goalNode);
		RoboNode nodeRight = addNodeIfValid(x + stepSize, y, path, goalNode);
		RoboNode nodeLeft = addNodeIfValid(x - stepSize, y, path, goalNode);

		if (nodeUp != null) {
			if (nodeLeft != null) {
				addNodeIfValid(x - stepSize, y + stepSize, path, goalNode);
			}
			if (nodeRight != null) {
				addNodeIfValid(x + stepSize, y + stepSize, path, goalNode);
			}
		}
		if (nodeDown != null) {
			if (nodeLeft != null) {
				addNodeIfValid(x - stepSize, y - stepSize, path, goalNode);
			}
			if (nodeRight != null) {
				addNodeIfValid(x + stepSize, y - stepSize, path, goalNode);
			}
		}
	
	}

	
    public GraphLogger getGraphLogger() {
		return graphLogger;
	}
    
	protected RoboNode addNodeIfValid(double x, double y, RoboPath path,
			RoboNode goalNode) {

		double speed = 1;
		if (x > xSize || y > ySize || x < -xSize || y < -ySize) {
			return null;
		}
		

		Point2d point = new Point2d(x, y);

		RoboMap.Content content = roboMap.getContentFromPosition(point
				, time + path.getDistanceToTarget() / speed);

		if (content != RoboMap.Content.OBJECT && !visited.contains(point)) {
			RoboNode node = new RoboNode(point, content);
			graphLogger.nodesCreated++;
			
			double distance = distance(node, goalNode);
			
			if(content == RoboMap.Content.MAYBEOBJECT) {
				distance += maybeObjectFactor;
			}
			
			RoboPath newPath = new RoboPath(path, node,
					distance / speed);
			graphLogger.pathsCreated += 1;
			queue.add(newPath);
			graphLogger.pathsAddedToQue++;

			return node;
		} else {
			return null;
		}

	}

	public Point2d[] findPath(RoboNode startNode, RoboNode goalNode) {
		queue.add(new RoboPath(null,startNode, distance(startNode,goalNode)));
		graphLogger.pathsAddedToQue++;
		
        while(!queue.isEmpty()) {
        	
        		graphLogger.iterationsInLoop++;
        	
                RoboPath currentPath = queue.poll();
                
                graphLogger.pathsPopedFromQueue++;
                
                if(visited.contains(currentPath.getLast().getPosition())) {
                	continue;
                }
                             
                RoboNode currentNode = currentPath.getLast();
                  
                visited.add(currentNode.getPosition());
                graphLogger.nodesVisited++;
                
                if(currentNode.equals(goalNode)) {
                    return currentPath.getPositions();
                }
                
                addValidNeighBoursToQueue(currentNode,currentPath,goalNode);
                
        }
        return null;
	}

	public static double distance(RoboNode n1, RoboNode n2) {

		return Math.sqrt(Math.pow(n1.getPosition().x - n2.getPosition().x, 2)
				+ Math.pow(n1.getPosition().y - n2.getPosition().y, 2));

	}

}
